\documentclass[a4paper,12pt]{article}
\usepackage{graphicx}
\usepackage{mathtools}
\usepackage{algorithmic}
\usepackage{algorithm}
\usepackage{tikz}
\usetikzlibrary{positioning,arrows}
\DeclareMathOperator*{\argmax}{arg\,max}
\begin{document}
\title{Restaurant Delivery Robots (RDR)}
\author{Tang Xiao, Huang Qi, Chong Chee Yong}
\maketitle
\begin{abstract}
In our project, we apply motion planning algorithms to Restaurant Delivery Robots (RDR), to enable them to deliver food like human waiters. To be more specific, after receiving a task, RDR goes to the food collection point, then routes to specific customers with the ability to avoid static as well as dynamic obstacles, and then returns to standby point. We also consider the case of slippery floor due to water spills, and RDR would attempt to avoid them if possible.
\end{abstract}

\section*{Restaurant}
The restaurant contains
\begin{itemize}
  \item Robot (red)
  \item Tables (orange)
  \item Person (green)
  \item Water spills (blue)
\end{itemize}

\section*{Controller}
The robot alternates between standby and delivery. 
\begin{algorithm}[H]
\caption{Control Loop}
\begin{algorithmic}[1]
  \LOOP
    \STATE generate orders
    \STATE deliver(orders)
    \STATE standby
  \ENDLOOP
\end{algorithmic}
\end{algorithm}

\subsection*{Standby}
The robot starts at the standby position.
This is the position where food is placed onto the robot for delivery.

\subsection*{Deliver}
The robot will deliver food to tables.
The goal position is a random position along the perimeter a short
distance away from the target table.

\section*{Plan and execute}
Whether delivering or going to standby, the robot
needs to perform planning and execution. The following types of algorithms are used:
\begin{itemize}
  \item Path Planning --- determine the way points to the goal before execution.
  \item Velocity Tuning --- determine how fast to move along path during execution.
\end{itemize}

\section*{Path Planning}
To plan the path, the robot needs to avoid the walls and tables.
Water spills are also avoided when possible.

Particle RRT \cite{pRRT} is used to perform the path planning.
The key idea of Particle RRT is to associate a probability of success
along the path generated by RRT. This probability of success is generated
by simulating a motion along the path.

\begin{algorithm}[H]
\caption{Plan Path}
\begin{algorithmic}[1]
  \STATE $bestProb \leftarrow 0$
  \REPEAT
    \STATE $path \leftarrow$ generate RRT path
    \STATE $prob \leftarrow$ evaluate $path$
    \IF{$prob > bestProb$}
      \STATE $bestProb \leftarrow prob$
      \STATE $bestPath \leftarrow path$
    \ENDIF
    \IF{$prob > threshold$}
      \RETURN $bestPath$
    \ENDIF
  \UNTIL timeout
  \RETURN $bestPath$
\end{algorithmic}
\end{algorithm}

\subsection*{Path Evaluation}
Path is evaluated by sampling the region of each RRT node for the presense of water. This is like taking a photograph of the particular area of the restaurant, and use some sampling algorithm to determine if the samples contain
water.

The probability of success of one node sample is
\[ p_{node} = \left\{
  \begin{array}{ll}
    1 & \text{ if there is no water}\\
    0.5 & \text{ if there is water}
  \end{array} \right. \]

This probability $p_{node}$ is affected by uncertainty (see Sensing Uncertainties).

The probability of success of $N$ node samples is the mean probability
\[ \hat{p_{node}} = \frac{1}{N} \sum_{node} p_{node} \]

The probability of success for the RRT path is the combined probability
\[ p_{path} = \prod_{node} \hat{p_{node}} \]

It is assumed that the water sensor permeates the entire restaurant, and
thus all probabilities are given equal weight.

\subsection*{Sensing Uncertainties}
To model uncertainty, there is a 0.2 probability of false positive or false negative. Hence it is possible for the robot to touch water.

\section*{Velocity Tuning}
Velocity tuning plans a velocity for moving along the path so that the robot does not collide into moving obstacles (people).

Equations that model the movements of the robot and the moving obstacles are first constructed, and then a constraint solver \cite{cpvt} is used to determine a feasible velocity.

\begin{algorithm}[H]
\caption{Execute Path}
\begin{algorithmic}[1]
  \REPEAT
    \STATE $curr \leftarrow$ get current position
    \STATE $next \leftarrow$ get next waypoint
    \STATE $segment \leftarrow$ pair($curr$, $next$)
    \STATE $obstacles \leftarrow$ get obstacle positions and velocities
    \STATE $v \leftarrow$ solve constraints ($segment$, $obstacles$)
    \STATE set robot velocity to $v$
  \UNTIL goal reached
\end{algorithmic}
\end{algorithm}

\subsection*{Constraints}
The constraints is (quoted from the paper) as follows:

\begin{itemize}
  \item Robot is moving between $V_{i-1} = (x_{i-1},y_{i-1})$ and $V_i = (x_i, y_i)$.
  \item Moving obstacle of size $S_x \times S_y$ is moving between $M_{j-1} = (x_{j-1},y_{j-1})$ and $M_j = (x_j, y_j)$, with velocity $v_j$, starting at time $t_{j-1}$.
\end{itemize}

Let
\begin{itemize}
  \item $(x_r,y_r)$ be the position of the robot,
  \item $(x_o,y_o)$ be the position of the moving obstacle,
  \item $t_c$ the collision time between them,
\end{itemize}
then the constraints for the robot moving on line segment $[V_{i-1},V_i]$, of
equation $a_i \cdot x_r + b_i \cdot c_i = 0$,
\[ \left\{
  \begin{array}{l}
    a_i = y_{i-1} - y_i\\
    b_i = x_i = x_{i-1}\\
    c_i = y_i \cdot x_{i-1} - x_i \cdot y_{i-1}\\
    a_i \cdot x_r + b_i \cdot y_r + c_i = 0\\
    (x_r - x_{i-1}) \cdot (x_r - x_i) \leq 0\\
    (y_r - y_{i-1}) \cdot (y_r - y_i) \leq 0
  \end{array} \right. \]

The moving obstacle moving on line segment $[M_{j-1},M_j]$ at velocity $v_j$, starting at time $t_{j-1}$, with distance traveled $d_j$,
\[ \left\{
  \begin{array}{l}
    d_{j_x} = x_j - x_{j-1}\\
    d_{j_y} = y_j - y_{j-1}\\
    d_j = \sqrt{d_{j_x}^2 + d_{j_y}^2}\\
    v_{j_x} = v_j \cdot (d_{j_x}/d_{j})\\
    v_{j_y} = v_j \cdot (d_{j_y}/d_{j})\\
    x_o = v_{j_x} \cdot (t_c - t_{j-1}) + x_{j-1}\\
    y_o = v_{j_y} \cdot (t_c - t_{j-1}) + y_{j-1}
  \end{array} \right. \]

The collision is the robot within the rectangular zone of the obstacle $S_x \times S_y$,
\[ \left\{
  \begin{array}{l}
    x_r \geq x_o + S_x / 2\\
    x_r \leq x_o - S_x / 2\\
    y_r \geq y_o + S_y / 2\\
    y_r \leq y_o - S_y / 2
  \end{array} \right. \]

\begin{thebibliography}{9}

\bibitem{pRRT}
   Nicholas Melchior and Reid Simmons,
   \emph{Particle RRT for Path Planning with Uncertainty},
   2007 IEEE International Conference on Robotics and Automation,
   April, 2007, pp. 1617-1624. 

\bibitem{cpvt}
   Micha\"{e}l Soulignac, Michel Rueher, Patrick Taillibert,
   \emph{A safe and flexible CP-based approach for velocity tuning problems},
   CP'10 Proceedings of the 16th international conference on Principles and practice of constraint programming,
   Pages 628-642 

\end{thebibliography}

\end{document}
